Controlling the Servo and Motors
Elias Groot
Software Lead, Course Organizer
To actually spin the motors and servo, you will need to write data to the official ASE actuator service. Based on our trajectory midpoint heuristics, we will write a steering value.
Creating a Write Stream
Similar to reading from the imaging service with a read stream, we want to write to the actuator service with a write stream. To understand to which stream to write, we turn to the actuator service's service.yaml definition and see which inputs it reads from:
...
inputs:
- service: controller
streams:
- decision
...
From the service "controller" it wants the stream "decision". Let's initialize this stream in our code, right after initializing the imaging read stream.
actuator := service.GetWriteStream("decision")
if actuator == nil {
return fmt.Errorf("Failed to create write stream 'decision'")
}
Notice that here we do not specify to which service we want to write to. We just tell the roverlib that "I want to write some data to a stream named 'decision'". We do not know nor check who will actually read from this stream.
Again similar to our imaging read stream, we have two methods available for writing data:
actuator.Write()which will take aSensorOutputmessage as Go struct, encode it to the binary wire format and send it outactuator.WriteBytes()which will take a raw write buffer from the caller and send it out
Writing To a Write Stream
Because we know that the actuator service expects a ControllerOutput message, using the actuator.Write() message is the obvious choice. Take a look at its definition here to understand which properties can be set for the actuator.
For now, let's only set the servo position, without spinning up the motors. To do so, we construct a ControllerOutput message first.
// Initialize the message that we want to send to the controller
actuatorMsg := pb_outputs.SensorOutput{
Timestamp: uint64(time.Now().UnixMilli()), // milliseconds since epoch
Status: 0, // all is well
SensorId: 1, // this is the first and only sensor we have
SensorOutput: &pb_outputs.SensorOutput_ControllerOutput{
ControllerOutput: &pb_outputs.ControllerOutput{
SteeringAngle: steerPosition,
LeftThrottle: 0,
RightThrottle: 0,
FanSpeed: 0,
FrontLights: false,
},
},
}
pb_outputs libraryThe pb_outputs library comes installed with roverlib-go, since it depends on the rovercom library. With it, we can instantiate protobuf messages as Go structs. Such libraries are often prefixed with pb_.
To import this Go library, use:
import (
pb_outputs "github.com/VU-ASE/rovercom/packages/go/outputs"
)
The nested pointer might seem complex at first sight. It unfortunately is a result of the Go typing system to allow for (somewhat complicated) union types. Also notice that we manually specify three general fields:
timestamp: when was this message generated? (In milliseconds since epoch)status: is this "sensor" still working well? (0 = all is well)sensorId: which sensor in this service did create this message? Must be non-empty. Useful if you have multiple sensors in the same sensor (for example, two cameras in one imaging service).
To send this SensorOutput message, we just use actuator.Write() like so:
// Send the message to the actuator
err = actuator.Write(&actuatorMsg)
if err != nil {
log.Warn().Err(err).Msg("Failed to send message to controller")
}
Convenient right? If we keep writing in a loop, we will continuously update our steering decisions that the actuator service will use to spin the servo and motors. At this point, your code should look like the following:
package main
import (
"fmt"
"os"
"time"
pb_outputs "github.com/VU-ASE/rovercom/packages/go/outputs"
roverlib "github.com/VU-ASE/roverlib-go/src"
"github.com/rs/zerolog/log"
)
// The main user space program
// this program has all you need from roverlib: service identity, reading, writing and configuration
func run(service roverlib.Service, configuration *roverlib.ServiceConfiguration) error {
// "I am the dummy controller, and I want to read from the 'path' stream from the 'imaging' service"
imaging := service.GetReadStream("imaging", "path")
if imaging == nil {
return fmt.Errorf("Failed to get stream 'path' from 'imaging' service")
}
actuator := service.GetWriteStream("decision")
if actuator == nil {
return fmt.Errorf("Failed to create write stream 'decision'")
}
for {
// Read one message from the stream
msg, err := imaging.Read()
if msg == nil || err != nil {
return fmt.Errorf("Failed to read from 'imaging' service")
}
// When did imaging service create this message?
createdAt := msg.Timestamp
// Convert epoch in milliseconds to a human readable format
createdAtHuman := time.Unix(0, int64(createdAt)/int64(time.Millisecond))
log.Info().Time("createdAt", createdAtHuman).Msg("Received message")
// Get the camera data
if msg.GetCameraOutput() == nil {
return fmt.Errorf("Message does not contain camera output. What did imaging do??")
}
cameraOutput := msg.GetCameraOutput()
log.Info().Msgf("imaging service captured a %d by %d image", cameraOutput.Trajectory.Width, cameraOutput.Trajectory.Height)
// This value holds the steering position that we want to pass to the servo (-1 = left, 0 = center, 1 = right)
steerPosition := float32(0)
// Find the desired mid point (x) of the captured image
desiredMidpoint := cameraOutput.Trajectory.Width / 2 // (cameraOutput.Trajectory.Width - 0) / 2
// Compute the actual mid point between the left and right edges of the detected track
if len(cameraOutput.Trajectory.Points) < 2 {
log.Debug().Msgf("Not enough track edge points to compute the mid point")
} else {
// Compute the mid point (assuming [0] is the left edge and [1[] is the right edge)
actualMidpoint := (cameraOutput.Trajectory.Points[1].X-cameraOutput.Trajectory.Points[0].X)/2 + cameraOutput.Trajectory.Points[0].X
// Compute the error
midpointErr := uint32(actualMidpoint) - desiredMidpoint
// Compute the steering position
steerPosition = float32(float64(midpointErr) / float64(desiredMidpoint))
}
// Initialize the message that we want to send to the controller
actuatorMsg := pb_outputs.SensorOutput{
Timestamp: uint64(time.Now().UnixMilli()), // milliseconds since epoch
Status: 0, // all is well
SensorId: 1, // this is the first and only sensor we have
SensorOutput: &pb_outputs.SensorOutput_ControllerOutput{
ControllerOutput: &pb_outputs.ControllerOutput{
SteeringAngle: steerPosition,
LeftThrottle: 0,
RightThrottle: 0,
FanSpeed: 0,
FrontLights: false,
},
},
}
// Send the message to the controller
err = actuator.Write(&actuatorMsg)
if err != nil {
log.Warn().Err(err).Msg("Failed to send message to controller")
}
}
}
// This function gets called when roverd wants to terminate the service
func onTerminate(sig os.Signal) error {
log.Info().Str("signal", sig.String()).Msg("Terminating service")
//
// ...
// Any clean up logic here
// ...
//
return nil
}
// This is just a wrapper to run the user program
// it is not recommended to put any other logic here
func main() {
roverlib.Run(run, onTerminate)
}
This already makes for a fully function (though quite dumb) controller service. Though, just like with declaring our inputs, we need to declare our output stream decision to roverd again through the service.yaml definition.
Declaring a Write Stream
Declaring a write stream through the service.yaml is even easier than declaring a read stream. You just need to specify the name of your stream in the outputs field, like so:
...
outputs:
- decision
...
Before you save, there is one more thing that we need to do. If you look at the actuator's service.yaml definition, you will see that it depends on stream decision from a service called controller. roverd will find and match this stream and service based on exact naming. Chances are that your service is currently named differently, so again turn to your service.yaml and make sure to adjust the name like so:
name: controller
...
This is all. Save all your changes and use roverctl to sync them if your Rover is powered on.